/*
 * TestForm.java
 *
 * Created on 13 de noviembre de 2007, 04:13 PM
 *
 * To change this template, choose Tools | Template Manager
 * and open the template in the editor.
 */

package Test;

import Behaviors.Arbitrators.*;
import Behaviors.Formations.*;
import Behaviors.Schemas.Motor.*;
import Behaviors.Schemas.Perception.*;

import Behaviors.Schemas.*;
import Controllers.*;
import Robot.*;
import Robot.Sensors.*;
import Utils.*;
import Behaviors.Schemas.Perception.PSDetectCentroid;


/**
 *
 * @author nxbot
 */
public class TestForm {
    
    /**
     * Creates a new instance of TestForm
     */
    public static void process(){
    
        //SimulationInterface simu=null;
        
        VirtualRobot robotLeader = new VirtualRobot("Virtual",5);
        VirtualPosition2d positionLeader = new VirtualPosition2d(robotLeader,"p",5);
        

        //***Definicion de Robots ****************************************************
        
        Robot robot1=new Robot("Robot 1","192.168.1.50",6665, 1);
        Robot robot2=new Robot("Robot 2","192.168.1.51",6665, 4);
        Robot robot3=new Robot("Robot 2","192.168.1.53",6665, 3);
        Robot robot4=new Robot("Robot 2","192.168.1.52",6665, 2);
        
        
       
        Robot camera1=new Robot("camera1","127.0.0.1",6665, 0);
        Robot camera2=new Robot("camera2","127.0.0.1",6667, 0);
        Robot camera3=new Robot("camera3","127.0.0.1",6668, 0);
        Robot camera4=new Robot("camera4","127.0.0.1",6666, 0);
        
         FusionPosition2d position1;
        try{
            position1=new FusionPosition2d(camera1,robot1,"position1",0);
        }
        catch(Exception e){
            e.printStackTrace();
            return;
        }
        
        
        FusionPosition2d position2;
        try{
            position2=new FusionPosition2d(camera2,robot2,"position2",0);
        }
        catch(Exception e){
            e.printStackTrace();
            return;
        }
        
        FusionPosition2d position3;
        try{
            position3=new FusionPosition2d(camera3,robot3,"position3",0);
        }
        catch(Exception e){
            e.printStackTrace();
            return;
        }
        
        FusionPosition2d position4;
        try{
            position4=new FusionPosition2d(camera4,robot4,"position4",0);
        }
        catch(Exception e){
            e.printStackTrace();
            return;
        }
        
        Blobfinder bf;
        try{
            bf = new Blobfinder(camera1,"bf",0);
        }
        catch(Exception e){
            e.printStackTrace();
            return;
        }
        

        

        //****************************************************************************

        
        robot1.runThreaded(10,0);
        robot2.runThreaded(10,0);
        robot3.runThreaded(10,0);
        robot4.runThreaded(10,0);
        
        camera1.runThreaded(10,0);
        camera2.runThreaded(10,0);
        camera3.runThreaded(10,0);
        camera4.runThreaded(10,0);

        
        //Formations to be used
        FormationSquare formQuad = new FormationSquare(4);
        FormationArrow formArrow = new FormationArrow(4);
        FormationRow formRow = new FormationRow (4);
        FormationColumn formCol = new FormationColumn (4);
        
        Formation[] array = {formQuad, formArrow, formRow, formCol};
        FormationGenerator formation=new FormationGenerator(array);
        
        positionLeader.setPosition(new Pose2d(0.0f,140.0f,(float)Math.toRadians(0.0f)));
        
        //positionLeader.setRotateVel(0.4f);
        
        Vector2d point=new Vector2d(0.0f,140.0f,Vector2d.MODE_XY);
        

        
        JFrameChangeForm frame = new JFrameChangeForm(formation,point,positionLeader);
     
        
        

        System.out.println("PointAngleA="+Float.toString((float)point.getAngle()));


        MSGoToPoint goto2 = new MSGoToPoint("goto2",positionLeader,point,MSGoToPoint.MODE_LINEAR);
        positionLeader.setAngControl(2.0f,-1);
        goto2.setMinDist(10);
        
        
        
        PSDetectFormation detectFormation1=new PSDetectFormation("goto2",formation,position1,positionLeader);
        PSDetectFormation detectFormation2=new PSDetectFormation("goto2",formation,position2,positionLeader);
        PSDetectFormation detectFormation3=new PSDetectFormation("goto2",formation,position3,positionLeader);
        PSDetectFormation detectFormation4=new PSDetectFormation("goto2",formation,position4,positionLeader);

        MSGoToPoint maintain1 = new MSGoToPoint("goto2",detectFormation1,position1,MSGoToPoint.MODE_EXPONENTIAL);
       MSGoToPoint maintain2 = new MSGoToPoint("goto2",detectFormation2,position2,MSGoToPoint.MODE_EXPONENTIAL);
        MSGoToPoint maintain3 = new MSGoToPoint("goto2",detectFormation3,position3,MSGoToPoint.MODE_EXPONENTIAL);
        MSGoToPoint maintain4 = new MSGoToPoint("goto2",detectFormation4,position4,MSGoToPoint.MODE_EXPONENTIAL);
       
        maintain1.setExponentialDistance(30);
        maintain2.setExponentialDistance(30);
        maintain3.setExponentialDistance(30);
        maintain4.setExponentialDistance(30);
       
       /* 
        Position2d [] positionsFor1={position3,position4};
        Position2d [] positionsFor2={position3,position4};
        Position2d [] positionsFor3={position1,position4};
        Position2d [] positionsFor4={position1,position3};
        */

            
        
        Position2d [] positionsFor1={position2,position3,position4};
        Position2d [] positionsFor2={position1,position3,position4};
        Position2d [] positionsFor3={position1,position2,position4};
        Position2d [] positionsFor4={position1,position2,position3};
        
        PSDetectObstaclesVirtualObj detectRobots1 = new PSDetectObstaclesVirtualObj("goto2",position1,positionsFor1,7);
        PSDetectObstaclesVirtualObj detectRobots2 = new PSDetectObstaclesVirtualObj("goto2",position2,positionsFor2,7);
        PSDetectObstaclesVirtualObj detectRobots3 = new PSDetectObstaclesVirtualObj("goto2",position3,positionsFor3,7);
        PSDetectObstaclesVirtualObj detectRobots4 = new PSDetectObstaclesVirtualObj("goto2",position4,positionsFor4,7);
        
        MSAvoidObstacles avoid1 = new MSAvoidObstacles("goto2",detectRobots1,position1,MSAvoidObstacles.MODE_EXPONENTIAL);
        MSAvoidObstacles avoid2 = new MSAvoidObstacles("goto2",detectRobots2,position2,MSAvoidObstacles.MODE_EXPONENTIAL);
        MSAvoidObstacles avoid3 = new MSAvoidObstacles("goto2",detectRobots3,position3,MSAvoidObstacles.MODE_EXPONENTIAL);
        MSAvoidObstacles avoid4 = new MSAvoidObstacles("goto2",detectRobots4,position4,MSAvoidObstacles.MODE_EXPONENTIAL);
        
        avoid1.setExponentialDistance(80);
        avoid2.setExponentialDistance(80);
        avoid3.setExponentialDistance(80);
        avoid4.setExponentialDistance(80);
        
        avoid1.setMaxRange(30);
        avoid2.setMaxRange(30);
        avoid3.setMaxRange(30);
        avoid4.setMaxRange(30);
        
        PSDetectObstaclesBlobfinder detectObst0 = new PSDetectObstaclesBlobfinder("goto2",positionLeader,bf,10);
        
        PSDetectObstaclesBlobfinder detectObst1 = new PSDetectObstaclesBlobfinder("goto2",position1,bf,5);
        PSDetectObstaclesBlobfinder detectObst2 = new PSDetectObstaclesBlobfinder("goto2",position2,bf,5);
        PSDetectObstaclesBlobfinder detectObst3 = new PSDetectObstaclesBlobfinder("goto2",position3,bf,5);
        PSDetectObstaclesBlobfinder detectObst4 = new PSDetectObstaclesBlobfinder("goto2",position4,bf,5);
        
        MSAvoidObstacles avoidO1 = new MSAvoidObstacles("goto2",detectObst1,position1,MSAvoidObstacles.MODE_EXPONENTIAL);
        MSAvoidObstacles avoidO2 = new MSAvoidObstacles("goto2",detectObst2,position2,MSAvoidObstacles.MODE_EXPONENTIAL);
        MSAvoidObstacles avoidO3 = new MSAvoidObstacles("goto2",detectObst3,position3,MSAvoidObstacles.MODE_EXPONENTIAL);
        MSAvoidObstacles avoidO4 = new MSAvoidObstacles("goto2",detectObst4,position4,MSAvoidObstacles.MODE_EXPONENTIAL);
        
        avoidO1.setExponentialDistance(80);
        avoidO2.setExponentialDistance(80);
        avoidO3.setExponentialDistance(80);
        avoidO4.setExponentialDistance(80);
        
        avoidO1.setMaxRange(10);
        avoidO2.setMaxRange(10);
        avoidO3.setMaxRange(10);
        avoidO4.setMaxRange(10);
        
        
        
        PSChangeFormation chform1 = new PSChangeFormation ("goto2",positionLeader, formation, detectObst0, null);
        
        Position2d [] positions={position1,position2,position3,position4};
        //Position2d [] positions={position1,position3,position4};
         
        PSDetectCentroid detectCentroid=new PSDetectCentroid("goto2",positions);
        MSGoToPoint gotoCent = new MSGoToPoint("goto2",detectCentroid,positionLeader,MSGoToPoint.MODE_EXPONENTIAL);
        gotoCent.setExponentialDistance(30);
        //goto2.setGain(0.8f);
        gotoCent.setMinDist(50);
        Schema[] arrayLeader = {goto2,chform1};//,gotoCent};//,chform1, aobstVirtual};//{chform1};//,aobst1};//  {maintain1, aobst1};
        
        Schema[] array1 = {maintain1,avoid1,avoidO1};//, aobst1};//{chform1};//,aobst1};//  {maintain1, aobst1};
        Schema[] array2 = {maintain2,avoid2,avoidO2};//, aobst2};// {chform2};//, aobst2};
        Schema[] array3 =  {maintain3,avoid3,avoidO3};//, aobst3};//{chform3};//, aobst3};
        Schema[] array4 =  {maintain4,avoid4,avoidO4};//, aobst4};//{chform4};//, aobst4};
        
        
        //MaintainFormation mantainFormation=new MaintainFormation(detectFormation);
        VectorAdditionArbitrator arbitratorLeader=new VectorAdditionArbitrator(arrayLeader,positionLeader, false);
        
        VectorAdditionArbitrator arbitrator1=new VectorAdditionArbitrator(array1,position1, false);
        VectorAdditionArbitrator arbitrator2=new VectorAdditionArbitrator(array2,position2, false);
        VectorAdditionArbitrator arbitrator3=new VectorAdditionArbitrator(array3,position3, false);
        VectorAdditionArbitrator arbitrator4=new VectorAdditionArbitrator(array4,position4, false);

        BehaviorBasedController controllerGoto=new BehaviorBasedController("goto",robotLeader,arbitratorLeader,50);
        
        BehaviorBasedController controller1=new BehaviorBasedController("Robot1 Controller",robot1,arbitrator1,50);
        BehaviorBasedController controller2=new BehaviorBasedController("Robot2 Controller",robot2,arbitrator2,50);
        BehaviorBasedController controller3=new BehaviorBasedController("Robot3 Controller",robot3,arbitrator3,50);
        BehaviorBasedController controller4=new BehaviorBasedController("Robot4 Controller",robot4,arbitrator4,50);
        
        
        //Position2d [] positions={position1,position2,position3,position4};
        //PSDetectCentroid detectCentroid=new PSDetectCentroid(positions);
        
        Errors errors=new Errors(formation,detectCentroid,positionLeader,positions);
        Thread hiloError=new Thread(errors);
        
        Thread hiloLeader=new Thread(controllerGoto);
        Thread hilo1=new Thread(controller1);
        Thread hilo2=new Thread(controller2);
        Thread hilo3=new Thread(controller3);
        Thread hilo4=new Thread(controller4);

        //positionLeader.setDebugMode(true);
        //arbitratorLeader.setDebugMode(true);
        frame.setVisible(true);

        hiloLeader.start();
        hilo1.start();
        hilo2.start();
        hilo3.start();
        hilo4.start();
        hiloError.start();

    }
}
